#include "ctrackerwindow.h"

#include <buola/gui/ctoolkit.h>
#include <buola/gui/cicon.h>
#include <buola/gui/cmenu.h>
#include <buola/gui/csimpleapp.h>
#include <buola/io/cfolder.h>
#include <buola/io/cresourcefinder.h>
#include <buola/image/algorithm.h>
#include <buola/image/io.h>
#include <buola/app/ceventloop.h>
#include <buola/mat/transformations.h>
#include <buola/cv/coccupancyoctree.h>
#include <buola/mat/cgaussianprocess.h>
#include <buola/app/ccmdline.h>
#include <buola/algorithm/comparison.h>
#include <buola/image/point.h>
#include <buola/scene/controllers/ccameracontroller.h>
#include <buola/scene/geodes/cpointcloud.h>
#include <buola/scene/cperspectivecamera.h>
#include <buola/widgets/ctoolbar.h>
#include <buola/threads/thread.h>
#include <buola/scene/transform/crttransform.h>
#include <buola/image/cgc.h>
#include <buola/scene/geodes/ccube.h>
#include <robot/nao/cnaoclient.h>

using namespace buola;

// static mat::CVec3d sStartPosition(0.037,0.230,1.322);
// static mat::CQuat_d sStartRotation(mat::C3DRotation(25.79_deg,45.88_deg,-147.8_deg));
static mat::CVec3d sStartPosition(0.688,0.484,2.124);
static mat::CQuat_d sStartRotation(mat::C3DRotation(168_deg,29_deg,-14_deg));
static mat::CVec3d sCameraOrigin(0,0,0);
static mat::CVec3d sCameraAt(0,0,1);
static mat::CVec3d sCameraUp(0,-1,0);

namespace buola { namespace tracker {

CTrackerWindow::CTrackerWindow()
    :   mTimer(250_ms)
    ,   mRecording(false)
{
    SetNumRows(2);
    mToolBar=new gui::CToolBar();
    Attach(mToolBar,gui::ATTACH_NORTH,gui::ATTACH_TOOLBAR);

    gui::PMenu lMenu=new gui::CMenu;
    
    mTimer.sTimer.Connect(&CTrackerWindow::NextSomething,this);

    lMenu->Add(gui::new_menu_item(L"next frame",ICON_STOCK_ARROW_RIGHT))->
              eTriggered.Connect([this]{mFrameNumber++;LoadFrame();});
    lMenu->Add(gui::new_menu_item(L"next iteration",ICON_STOCK_ARROW_RIGHT))->
              eTriggered.Connect(&CTrackerWindow::NextIteration,this,true,1);
    lMenu->Add(gui::new_menu_item(L"25 iterations",ICON_STOCK_ARROW_RIGHT_DOUBLE))->
              eTriggered.Connect(&CTrackerWindow::NextIteration,this,true,25);
    lMenu->Add(gui::new_menu_item(L"run",ICON_STOCK_RUN))->
              eTriggered.Connect([this]{mTimer.Toggle();});
    lMenu->Add(gui::new_menu_item(L"record",ICON_STOCK_HELP))->
              eTriggered.Connect([this]{mRecording=!mRecording;});
    
    mToolBar->SetMenu(lMenu);

    SetupCamera();
    SetupScene();
    SetupViews();
    SetupInput();

    AddCommitHook([this](const CRegion &pRegion){OnCommitHook(pRegion);});
}

CTrackerWindow::~CTrackerWindow()
{
}

#if 0
void CTrackerWindow::SetupScene()
{
    (new scene::CCube(0.228,0.189,0.04))->AttachTo(mVVSTransform);
}
#endif

#if 1
void CTrackerWindow::SetupScene()
{
    mVVSTransform->AddChild(mVirtualNao.GetRootTransform());
}
#endif

void CTrackerWindow::SetupCamera()
{
    mScene=new scene::CScene;
    mVVSTransform=new scene::CRTTransform(mat::CQuat_d(),mat::CVec3d(0,0,0));
    mScene->World()->AddChild(mVVSTransform);

    mCamera=new scene::CPerspectiveCamera;
    mCamera->SetClipping(0.100,10);
    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
    mCamera->SetShift(lParams.GetC()-CSize_d(320,240),CSize_d(640,480));
    mCamera->SetFOV(2*atan2(240,lParams.GetF().y));
    mCamera->LookAt(sCameraAt,sCameraOrigin,sCameraUp);
    
    mVVS.SetScene(mScene,mVVSTransform);
    mVVS.SetCamera(mCamera);
}


void CTrackerWindow::SetupViews()
{
    mSceneView=new scene::CSceneView;
    
    mSceneView->AddScene(mScene);
    scene::PPerspectiveCamera lCamera=new scene::CPerspectiveCamera;
    lCamera->SetFOV(2*atan2(240.0,525.0));
    lCamera->SetClipping(0.100,10);
    lCamera->LookAt(sCameraAt,sCameraOrigin,sCameraUp);
    mSceneView->SetCamera(lCamera);
    mSceneView->AddController(new scene::CCameraController(lCamera));
    mSceneView->Resize({640,480});
    mSceneView->SetTitle(L"current iteration");
    
    scene::PScene lScene=new scene::CScene;
    mSceneCloud=new scene::CPointCloud;
    lScene->World()->AddObject(mSceneCloud);
    mSceneView->AddScene(lScene);

    Add(mSceneView);
    
    mNumbersBox=new gui::CListContainer;
    mNumbersBox->AddGroup(L"pose",0);
    mNumbersBox->AddGroup(L"method",1);
    mNumbersBox->Resize({200,480});
    for(int i=0;i<6;i++)
    {
        mCoordsBox[i]=new gui::CNumBox;
        mCoordsBox[i]->Resize({200,20});
        mNumbersBox->Add(mCoordsBox[i],0);
        if(i<3)
            mCoordsBox[i]->SetStep(0.1);
        else
            mCoordsBox[i]->SetStep(2.0);
        mCoordsBox[i]->SetStyle(gui::CNumBox::STYLE_DOUBLE);
        mCoordsBox[i]->eNumChange.Connect(&CTrackerWindow::OnCoordsBox,this,i);
    }

    mCoordsBox[0]->SetTitle(L"x");
    mCoordsBox[1]->SetTitle(L"y");
    mCoordsBox[2]->SetTitle(L"z");
    mCoordsBox[3]->SetTitle(L"a");
    mCoordsBox[4]->SetTitle(L"b");
    mCoordsBox[5]->SetTitle(L"c");

//    Add(mNumbersBox);
}

void CTrackerWindow::SetupInput()
{
    mVVS.SetInitialPose(sStartPosition,sStartRotation);
    
    mTargetImage.Recreate({640,480});
    
    LoadFrame();
}

void CTrackerWindow::LoadFrame()
{
    lock_guard<recursive_mutex> lLock(get_main_event_loop().GetMutex());

    img::CImage_gray32f lTargetDepth;
    
    img::load(lTargetDepth,io::file("/home/xavi/naos/depth_"+to_string(mFrameNumber)+".tif"));
    img::load(mTargetImage,io::file("/home/xavi/naos/img_"+to_string(mFrameNumber)+".ppm"));

    img::CCameraParameters lParams;
    lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));

    mVVS.SetTarget(mTargetImage,lTargetDepth);
    mSceneCloud->SetCloud(cvi::make_cloud(lTargetDepth,mTargetImage,lParams));
    
    if(mImageBox[0])
        mImageBox[0]->Refresh();
    
    io::CTextReader lIS(io::file("/home/xavi/naos/nao_"+to_string(mFrameNumber)));
    mat::CVecNd<26> lAngles;
    lIS >> lAngles;
    mVirtualNao.SetPose(lAngles);
    mVirtualNao.Update();
    
    mSceneView->Refresh();
}

void CTrackerWindow::NextIteration(bool pReally,int pCount)
{
    for(int i=0;i<pCount;i++)
        mVVS.Iterate(pReally,true,true);
 
    mDirectionsImage.Recreate(mTargetImage.Size());
    mVVS.GetDirectionsImage(mDirectionsImage);

    auto lInputImage=clone(mTargetImage);
    mVVS.DrawRenderEdges(lInputImage);

    if(!mImageBox[0])
    {
        for(int i=0;i<3;i++)
        {
            mImageBox[i]=new gui::CImageBox;
            mImageBox[i]->SetTitle(L"something");
            Add(mImageBox[i]);
        }

        mImageBox[0]->SetTitle(L"input");
        mImageBox[1]->SetTitle(L"render");
        mImageBox[2]->SetTitle(L"edges");
    }

    mImageBox[0]->SetImage(std::move(lInputImage));
    mImageBox[1]->SetImage(clone(mVVS.LastRender()));
    mImageBox[2]->SetImage(clone(mDirectionsImage));
    mSceneView->Refresh();
    
    mat::CVec3d lPosition=mVVS.Position();
    mat::C3DRotation lRotation(mVVS.Rotation());
    
    mCoordsBox[0]->SetValue(lPosition.x());
    mCoordsBox[1]->SetValue(lPosition.y());
    mCoordsBox[2]->SetValue(lPosition.z());
    mCoordsBox[3]->SetValue(rad2deg(lRotation.a));
    mCoordsBox[4]->SetValue(rad2deg(lRotation.b));
    mCoordsBox[5]->SetValue(rad2deg(lRotation.c));
}

void CTrackerWindow::NextSomething()
{
    if(mIterationNumber>1)
    {
        mFrameNumber++;
        if(mFrameNumber==100249)
        {
            mTimer.Disable();
            return;
        }
        LoadFrame();
        mIterationNumber=0;
    }
    
    mIterationNumber++;
    NextIteration(true,25);
}

void CTrackerWindow::OnCoordsBox(int i)
{
    mat::CVec3d lPosition(mCoordsBox[0]->GetDoubleValue(),mCoordsBox[1]->GetDoubleValue(),mCoordsBox[2]->GetDoubleValue());
    mat::C3DRotation lRotation=deg2rad(mat::C3DRotation(mCoordsBox[3]->GetDoubleValue(),mCoordsBox[4]->GetDoubleValue(),mCoordsBox[5]->GetDoubleValue()));
           
    mVVS.SetInitialPose(lPosition,mat::CQuat_d(lRotation));
    NextIteration(false);
    
    mSceneView->Refresh();
}

void CTrackerWindow::OnCommitHook(const CRegion& pRegion)
{
    if(!mRecording) return;
    CSize_i lSize((int)Size().x/4*4,(int)Size().y/4*4);
    img::CImage_rgb8 lImage(lSize);
     
    start_timer();
    glReadBuffer(GL_FRONT);
    glReadPixels(0,0,lSize.x,lSize.y,GL_RGB,GL_UNSIGNED_BYTE,lImage.RawData());
    glReadBuffer(GL_BACK);
    end_timer();
    
    img::save(lImage,io::file("/tmp/commit"+to_string(mOutNumber++)+".ppm"));
}


/*namespace tracker*/ } /*namespace buola*/ }

int main(int pNArgs,char **pArgs)
{
    using namespace buola;
    
    buola_init(pNArgs,pArgs);

    try
    {
        gui::get_toolkit()->GetOpenGLGC()->MakeCurrent();
        gui::CSimpleApp<tracker::CTrackerWindow> lApp;
        lApp.Run();
    }
    catch(std::exception &pE)
    {
        msg_error() << pE.what() << "\n";
    }

    return buola_finish();
}
